//
// Created by xuan on 24-1-18.
//
#include "UPRE_DIJKSTRA.h"
#include <stdio.h>
#include <limits.h>
#include <stdbool.h>
#include "string.h"
#include "UPRE_Task_Flow.h"
//#include "cmsis_os.h"
//niuma

#define GET_ACC 5
#define GET_DES 70
#define GET_START_SPD 8000
#define GET_MAX_SPD   10000
#define PUT_ACC 5
#define PUT_DES 70
#define PUT_START_SPD 7500
#define PUT_MAX_SPD   12500
#define NORMAL_YAW 40

extern uint8_t Loop_Num;
extern uint8_t Knots_Num;

////0:紫色有球 1：蓝色有球 2：紫色无球 3：蓝色无球
uint8_t ball[Ball_Num_Line][Ball_Num_Column] = {
    {0, 0, 0, 0, 0, 0},
    {0, 0, 0, 0, 0, 0}
};
int PathKnots[V] = {0};
///0:蓝方 1：红方
const Knots_state Actual_point_Bule[V] = {
    /////TODO:取球
    {4864.f, 3545.f, 0.f, {0}, {0}, 0, 0, 0, 0, 0, 0}, ///0
    {4864.f, 3049.f, 0.f, {0}, {0}, 0, 0, 0, 0, 0, 0}, ///1
    {4864.f, 2541.f, 0.f, {0}, {0}, 0, 0, 0, 0, 0, 0}, ///2
    {4864.f, 2058.f, 0.f, {0}, {0}, 0, 0, 0, 0, 0, 0}, ///3
    {4864.f, 1563.f, 0.f, {0}, {0}, 0, 0, 0, 0, 0, 0}, ///4
    {4864.f, 1001.f, 0.f, {0}, {0}, 0, 0, 0, 0, 0, 0}, ///5
    /////TODO:↑第一排球 ↓第二排球
    {4864.f, 3545.f, 0.f, {0}, {0}, 0, 0, 0, 0, 0, 0}, ///6 0
    {4864.f, 3049.f, 0.f, {0}, {0}, 0, 0, 0, 0, 0, 0}, ///7 1
    {4864.f, 2541.f, 0.f, {0}, {0}, 0, 0, 0, 0, 0, 0}, ///8 2
    {4864.f, 2058.f, 0.f, {0}, {0}, 0, 0, 0, 0, 0, 0}, ///9 3
    {4864.f, 1563.f, 0.f, {0}, {0}, 0, 0, 0, 0, 0, 0}, ///10 4
    {4864.f, 1001.f, 0.f, {0}, {0}, 0, 0, 0, 0, 0, 0}, ///11 5
    /////TODO:↓运动路径 ↑没用了
    {4864.f, 3560.f, 0.f, {0}, {0}, 1500, 1500, 0, 0, 5, 0},
    ///12 0 //取球
    {4864.f, 3049.f, 0.f, {0}, {0}, 1500, 1500, 0, 0, 5, 0},
    ///13 1 //取球
    {4844.f, 2566.f, 0.f, {0}, {0}, 1500, 1500, 0, 0, 5, 0},
    ///14 2 //取球
    {4844.f, 2074.f, 0.f, {0}, {0}, 1500, 1500, 0, 0, 5, 0},
    ///15 3 //取球
    {4844.f, 1563.f, 0.f, {0}, {0}, 1500, 1500, 0, 0, 5, 0},
    ///16 4 //取球
    {4844.f, 1066.f, 0.f, {0}, {0}, 1500, 1500, 0, 0, 5, 0},
    ///17 5 //取球
    {4844.f, 519.f, 0.f, {5513.f, 219.f}, {5513.f, 219.f}, 11000, 11000, 0, 15, 15, 40},
    ///18//过渡，准备取球
    {4100.f, 519.f, 0.f, {4958.f, -744.2f}, {4958.f, -331.4f}, 11000, 11000, 0, 15, 15, 0},
    ///19//上坡完成
    /////TODO::放苗
    {
        2223.f, -797.f, -90.f, {2908.f, -957.f}, {3358.f, -957.f}, GET_START_SPD, GET_MAX_SPD, 0, GET_ACC, GET_DES,
        NORMAL_YAW
    }, ///20//上坡
    {
        2230.f, 632.f, -90.f, {2274.f, -294.6f}, {2274.f, -831.f}, 3500, 3500, 0, GET_ACC, 50,
        NORMAL_YAW
    }, ///21//放
    {
        1979.f, 622.f, -90.f, {791.f, 734.f}, {1800.f, 832.f}, GET_START_SPD, GET_MAX_SPD, 0, GET_ACC, GET_DES,
        NORMAL_YAW
    }, ///22//过渡
    {
        1979.f, 1110.f, -90.f, {791.f, 734.f}, {1800.f, 832.f}, GET_START_SPD, GET_MAX_SPD, 0, GET_ACC, GET_DES,
        NORMAL_YAW
    }, ///23//放
    {
        1976.f, 1615.f, -90.f, {2268.f, 451.f}, {619.f, 913.f}, 3500, 3500, 0, GET_ACC, 50,
        NORMAL_YAW
    }, ///24//放
    {
        2246.f, 1615.f, -90.f, {782.f, 1130.f}, {1751.f, 1098.f}, GET_START_SPD, GET_MAX_SPD, 0, GET_ACC, GET_DES,
        NORMAL_YAW
    }, ///25//过渡
    {
        2246.f, 2102.f, -90.f, {782.f, 1130.f}, {1751.f, 1098.f}, GET_START_SPD, GET_MAX_SPD, 0, GET_ACC, GET_DES,
        NORMAL_YAW
    }, ///26//放
    {
        2251.f, 2615.f, -90.f, {2269.f, 1026.f}, {509.f, 1178.f}, 3500, 3500, 0, GET_ACC, 50,
        NORMAL_YAW
    }, ///27//放
    {
        2045.f, 2615.f, -90.f, {819.f, 1835.f}, {1720.f, 1735.f}, GET_START_SPD, GET_START_SPD, 0, GET_ACC, GET_DES,
        NORMAL_YAW
    }, ///28//过渡
    {
        2035.f, 3126.f, -90.f, {819.f, 1835.f}, {1720.f, 1735.f}, 15000, 18000, 0, 1, 99,
        NORMAL_YAW
    }, ///29//放
    ///TODO::取苗
    {186.f, 3333.f, 0.f, {445.f, 2754.f}, {773.f, 3431.f}, 15000, 18000, 0, 1, 99, NORMAL_YAW},
    ///30//取3
    {186.f, 2332.f, 0.f, {448.f, 1692.f}, {760.f, 2451.f}, 15000, 18000, 0, 1, 99, NORMAL_YAW},
    ///31//取2
    {185.f, 1353.f, 0.f, {726.f, -110.f}, {673.f, 1458.f}, 15000, 18000, 0, 1, 99, NORMAL_YAW},
    ///32//取1
    // {0.0f, 3000.f, 0.f, {308.f, 327.f}, {530.f, 839.f}, 20000, 20000, 20000, 5, GET_DES}, ///29
    // /////TODO:Start
    {0.f, 0.f, 0.f, {0}, {0}, 0, 0, 0, 0, 0},
    ///33//起点
    {4100.f, -797.f, -90.f, {0}, {0}, GET_START_SPD, GET_MAX_SPD, 0, GET_ACC, GET_DES,0}
    ///34//二区避开R2重试区过度点
    /////TODO:以下没用
};


const Knots_state Actual_point_Red[V] = {
    /////TODO:取球
    {4864.f, -3545.f, 0.f, {0}, {0}, 1000, 1000, 1000, 0, 0, 1}, ///0
    {4864.f, -3049.f, 0.f, {0}, {0}, 1000, 1000, 1000, 0, 0, 1}, ///1
    {4864.f, -2541.f, 0.f, {0}, {0}, 1000, 1000, 1000, 0, 0, 1}, ///2
    {4864.f, -2058.f, 0.f, {0}, {0}, 1000, 1000, 1000, 0, 0, 1}, ///3
    {4864.f, -1563.f, 0.f, {0}, {0}, 1000, 1000, 1000, 0, 0, 1}, ///4
    {4864.f, -1001.f, 0.f, {0}, {0}, 1000, 1000, 1000, 0, 0, 1}, ///5
    /////TODO:↑第一排球 ↓第二排球
    {4864.f, -3545.f, 0.f, {0}, {0}, 1000, 1000, 1000, 0, 0, 1}, ///6 0
    {4864.f, -3049.f, 0.f, {0}, {0}, 1000, 1000, 1000, 0, 0, 1}, ///7 1
    {4864.f, -2541.f, 0.f, {0}, {0}, 1000, 1000, 1000, 0, 0, 1}, ///8 2
    {4864.f, -2058.f, 0.f, {0}, {0}, 1000, 1000, 1000, 0, 0, 1}, ///9 3
    {4864.f, -1563.f, 0.f, {0}, {0}, 1000, 1000, 1000, 0, 0, 1}, ///10 4
    {4864.f, -1001.f, 0.f, {0}, {0}, 1000, 1000, 1000, 0, 0, 1}, ///11 5
    /////TODO:↓运动路径
    {4864.f, -3545.f, 0.f, {0}, {0}, 1000, 1000, 1000, 0, 0, 1}, ///12 0
    {4864.f, -3049.f, 0.f, {0}, {0}, 1000, 1000, 1000, 0, 0, 1}, ///13 1
    {4864.f, -2541.f, 0.f, {0}, {0}, 1000, 1000, 1000, 0, 0, 1}, ///14 2
    {4864.f, -2058.f, 0.f, {0}, {0}, 1000, 1000, 1000, 0, 0, 1}, ///15 3
    {4864.f, -1563.f, 0.f, {0}, {0}, 1000, 1000, 1000, 0, 0, 1}, ///16 4
    {4864.f, -1001.f, 0.f, {0}, {0}, 1000, 1000, 1000, 0, 0, 1}, ///17 5
    {4864.f, -219.f, 0.f, {5513.f, -219.f}, {5513.f, -219.f}, 11000, 11000, 0, 15, 15}, ///18
    {4970.f, 750.f, 90.f, {4958.f, 744.2f}, {4958.f, 331.4f}, 11000, 11000, 0, 15, 15}, ///19
    /////TODO::放苗
    {
        2161.f, 750.f, 90.f, {2908.f, 957.f}, {3358.f, 957.f}, GET_START_SPD, GET_MAX_SPD, 0, GET_ACC, GET_DES,
        NORMAL_YAW
    }, ///20//上坡
    {
        2161.f, -668.f, 90.f, {2274.f, 294.6f}, {2274.f, 831.f}, 2000, 2000, 0, GET_ACC, 50,
        NORMAL_YAW
    }, ///21//放
    {
        1921.f, -668.f, 90.f, {791.f, 734.f}, {1800.f, -832.f}, GET_START_SPD, GET_MAX_SPD, 0, GET_ACC, GET_DES,
        NORMAL_YAW
    }, ///22//过渡
    {
        1921.f, -1112.f, 90.f, {791.f, -734.f}, {1800.f, -832.f}, GET_START_SPD, GET_MAX_SPD, 0, GET_ACC, GET_DES,
        NORMAL_YAW
    }, ///23//放
    {
        1928.f, -1659.f, 90.f, {2268.f, -451.f}, {619.f, -913.f}, 2000, 2000, 0, GET_ACC, 50,
        NORMAL_YAW
    }, ///24//放
    {
        2186.f, -1659.f, 90.f, {782.f, -1130.f}, {1751.f, -1098.f}, GET_START_SPD, GET_MAX_SPD, 0, GET_ACC, GET_DES,
        NORMAL_YAW
    }, ///25//过渡
    {
        2186.f, -2181.f, 90.f, {782.f, -1130.f}, {1751.f, -1098.f}, GET_START_SPD, GET_MAX_SPD, 0, GET_ACC, GET_DES,
        NORMAL_YAW
    }, ///26放
    {
        2167.f, -2683.f, 90.f, {2269.f, -1026.f}, {509.f, -1178.f}, 2000, 2000, 0, GET_ACC, 50,
        NORMAL_YAW
    }, ///27//放
    {
        1897.f, -2683.f, 90.f, {819.f, -1835.f}, {1720.f, -1735.f}, GET_START_SPD, GET_START_SPD, 0, GET_ACC, GET_DES,
        NORMAL_YAW
    }, ///28//过渡
    {
        1897.f, -3190.f, 90.f, {819.f, -1835.f}, {1720.f, -1735.f}, GET_START_SPD, GET_MAX_SPD, 0, GET_ACC, GET_DES,
        NORMAL_YAW
    }, ///29//放
    ///TODO::取苗j
    {
        137.f, -3345.f, 0.f, {540.f, -2989.f}, {619.f, -3433.f}, 15000, 18000, 0, GET_ACC, GET_DES,
        NORMAL_YAW
    },
    ///30//取3
    {
        153.f, -2345.f, -1.f, {448.f, -1692.f}, {760.f, -2451.f}, 15000, 18000, 0, GET_ACC, GET_DES,
        NORMAL_YAW
    },
    ///31//取2
    {
        178.f, -1357.f, -1.f, {726.f, 110.f}, {673.f, -1458.f}, 15000, 18000, 0, GET_ACC, GET_DES,
        NORMAL_YAW
    },
    ///32//取1
    // {0.0f, 3000.f, 0.f, {308.f, 327.f}, {530.f, 839.f}, 20000, 20000, 20000, 5, GET_DES}, ///29
    // /////TODO:Start
    {0.f, 0.f, 0.f, {0}, {0}, 0, 0, 0, 0, 0},
    ///33//起点
    {0.f, 0.f, 0.f, {0}, {0}, 0, 0, 0, 0, 0}
    ///34//二区避开R2重试区过度点
    /////TODO:以下没用
};

//////节点的邻接矩阵
int graph[V][V] = {
    {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, //0
    {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, //1
    {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, //2
    {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, //3
    {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, //4
    {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, //5
    {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, //6
    {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, //7
    {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, //8
    {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, //9
    {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, //10
    {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, //11
    {1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, //12
    {0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, //13
    {0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, //14
    {0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, //15
    {0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, //16
    {0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, //17
    {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, //18
    {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0}, //19
    {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0}, //20
    {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, //21
    {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, //22
    {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, //23
    {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, //24
    {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0}, //25
    {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0/*1*/, 0, 0, 0, 0},
    //26
    {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0}, //27
    {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 0}, //28
    {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0}, //29
    {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0}, //30
    {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0}, //31
    {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0}, //32
    {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0/*1*/, 0, 0, 0, 0, 0, 1, 0, 0, 0},
    //33
    {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, //34
    {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0} //35

};


static int PathCount = 0;

int Dijkstra_CountKnots = 0;

bool Flag_OnceCalculateDijkstra = true;

static bool Last_color = true; /////0:紫 1：蓝

static int minDistance(int dist[], int sptSet[]) {
    int min = INT_MAX, min_index;

    for (int v = 0; v < V; v++) {
        if (sptSet[v] == 0 && dist[v] <= min) {
            min = dist[v];
            min_index = v;
        }
    }

    return min_index;
}

static void printPath(int parent[], int j) {
    if (parent[j] == -1) {
        return;
    }
    printPath(parent, parent[j]);
    //    printf("%d ", j);
    PathKnots[PathCount] = j;
    PathCount++;
}

static int dijkstra(int src, int dest) {
    int sptSet[V];
    int parent[V];

    for (int i = 0; i < V; i++) {
        sptSet[i] = 0;
        parent[i] = -1;
    }

    int dist[V];
    for (int i = 0; i < V; i++) {
        dist[i] = INT_MAX;
    }

    dist[src] = 0;
    for (int count = 0; count < V - 1; count++) {
        int u = minDistance(dist, sptSet);

        sptSet[u] = 1;

        for (int v = 0; v < V; v++) {
            if (!sptSet[v] && graph[u][v] && dist[u] != INT_MAX &&
                dist[u] + graph[u][v] < dist[v]) {
                dist[v] = dist[u] + graph[u][v];
                parent[v] = u;
            }
        }
    }

    //    printf("顶点 \t 最短路径 \t 路径长度\n");
    //    printf("%d -> %d \t\t ", src, dest);
    printPath(parent, dest);
    PathCount = 0;
    // 动态计算路径长度
    int pathLength = 0;
    int currentNode = dest;
    while (parent[currentNode] != -1) {
        pathLength += graph[parent[currentNode]][currentNode];
        currentNode = parent[currentNode];
    }

    //    printf("\t\t%d\n", pathLength);
    return pathLength;
}

static bool check_Knots(int Knots) {
    if (Last_color) /////上一次取了篮球，这一次要取紫球
    {
        if ((ball[Knots / 6][Knots % 6]) == 0) {
            return true;
        }
    } else /////上一次取了紫球，这一次要取篮球
    {
        if ((ball[Knots / 6][Knots % 6]) == 1) {
            return true;
        }
    }
    return false;
}

static int findMinIndex(const int arr[], int size) {
    if (size <= 0) {
        // 处理空数组或无效输入的情况
        return -1;
    }

    int minIndex = 0; // 假设初始最小值的索引为第一个元素的索引

    for (int i = 1; i < size; i++) {
        // 逐个比较，更新最小值的索引
        if (arr[i] < arr[minIndex]) {
            minIndex = i;
        }
    }

    return minIndex;
}

static int findMinIndex_F(const float arr[], int size) {
    if (size <= 0) {
        // 处理空数组或无效输入的情况
        return -1;
    }

    int minIndex = 0; // 假设初始最小值的索引为第一个元素的索引

    for (int i = 1; i < size; i++) {
        // 逐个比较，更新最小值的索引
        if (arr[i] < arr[minIndex]) {
            minIndex = i;
        }
    }

    return minIndex;
}

static bool areArraysEqual(int arr1[], int arr2[], int size) {
    for (int i = 0; i < size; ++i) {
        if (arr1[i] != arr2[i]) {
            return false; // 返回0表示数组不相等
        }
    }
    return true; // 返回1表示数组相等
}

static int cheese_Knots(int current_knots) {
    int distance[12] = {0};
    int MAX_Distance[12] = {0};
    for (int i = 0; i < 12; ++i) {
        MAX_Distance[i] = INT_MAX;
    }
    if (current_knots >= 0 && current_knots <= 11) {
        if (current_knots <= 5) {
            memset(PathKnots, -1, sizeof(PathKnots));
            return (current_knots + 12);
        } else {
            memset(PathKnots, -1, sizeof(PathKnots));
            return (current_knots + 6);
        }
    } else if (current_knots >= 12) {
        for (int i = 0; i < 12; i++) {
            if (check_Knots(i)) {
                distance[i] = dijkstra(current_knots, i);
                memset(PathKnots, -1, sizeof PathKnots);
            } else {
                distance[i] = INT_MAX;
                continue;
            }
        }
        if (areArraysEqual(MAX_Distance, distance, 12))
            return V;
        else
            return (findMinIndex(distance, 12));
    } else
        return V;
}

bool Dijkstra_PathPlanning(int CurrentKnots) {
    int NextKnots, distance, Count = 0;
    NextKnots = cheese_Knots(CurrentKnots);
    if (NextKnots >= V) {
        printf("DIJKSTRA END!!!!!!!!!!!!\r\n");
        return false;
    } else {
        distance = dijkstra(CurrentKnots, NextKnots);
        //    printf("Current:%d,Next:%d,distance:%d\t Path: ",CurrentKnots,NextKnots,distance);
        printf("Current:%d,Next:%d,distance:%d\r\n", CurrentKnots, NextKnots, distance);
        for (int i = 0; PathKnots[i] >= 0; ++i) {
            printf(" %d", PathKnots[i]);
            Count++;
        }
        printf("\r\n");
        if (CurrentKnots >= 12 && CurrentKnots <= 18) {
            ball[(NextKnots) / 6][(NextKnots) % 6] = ball[(NextKnots) / 6][(NextKnots) % 6] + 2;
            if (Last_color)
                Last_color = false;
            else
                Last_color = true;
        }
        Dijkstra_CountKnots = Count;
        Count = 0;
        return true;
    }
}

bool Dijkstra_FirstPathPlanning(int CurrentKnots) {
    int NextKnots, distance, Count = 0;
    NextKnots = 18;
    memset(PathKnots, -1, sizeof PathKnots);
    distance = dijkstra(CurrentKnots, NextKnots);
    printf("Current:%d,Next:%d,distance:%d\t Path: ",CurrentKnots,NextKnots,distance);
    // printf("Current:%d,Next:%d,distance:%d\r\n", CurrentKnots, NextKnots, distance);
    for (int i = 0; PathKnots[i] >= 0; ++i) {
        printf(" %d ", PathKnots[i]);
        Count++;
    }
    printf("\r\n");
    Dijkstra_CountKnots = Count;
    Count = 0;

    Knots_Num=0;
    Loop_Num=0;
    return true;
}

static void Proximity_Weight(float *knots_weight, Pose courent_pose) {
    float weiget = 0.0f;
    float distance = 0.0f;

    for (int i = 0; i < V; ++i) {
        if (TaskFlag.Flag_BlueZone && !TaskFlag.Flag_RedZone) {
            distance = sqrt(
                ((courent_pose.wx - Actual_point_Bule[i].wx) * (courent_pose.wx - Actual_point_Bule[i].wx)) + (
                    (courent_pose.wy - Actual_point_Bule[i].wy) * (courent_pose.wy - Actual_point_Bule[i].wy)));
        } else if (TaskFlag.Flag_RedZone && !TaskFlag.Flag_BlueZone) {
            distance = sqrt(
                ((courent_pose.wx - Actual_point_Red[i].wx) * (courent_pose.wx - Actual_point_Red[i].wx)) + (
                    (courent_pose.wy - Actual_point_Red[i].wy) * (courent_pose.wy - Actual_point_Red[i].wy)));
        }
        weiget = 1 / distance;
        knots_weight[i] = weiget;
    }
}

unsigned char ManualToAutoMation_FindNearNode(Pose courent_pose) {
    unsigned char knot = 0;
    float knots_weight[V] = {0};

    Proximity_Weight(knots_weight, courent_pose);

    knot = findMinIndex_F(knots_weight, sizeof knots_weight);

    if (knot <= 18) {
        knot = 18;
    }

    return knot;
}

bool DIJKSTRA_CheckBall(void) {
    int blue_Num = 0, Purple_Num = 0;
    for (int i = 0; i < 12; i++) {
        if (ball[i / 6][i % 6] == 0)
            Purple_Num++;
        else if (ball[i / 6][i % 6] == 1) {
            blue_Num++;
        }
    }
    if (blue_Num == Purple_Num) {
        return true;
    } else {
        return false;
    }
}

void Dijkstra_DijkstraCalculate(void) {
    if (Dijkstra_PathPlanning(PathKnots[Dijkstra_CountKnots - 1])) {
        Knots_Num = 0;
        Loop_Num = 0;
        // Flag_OnceCalculateDijkstra = false;
        printf("Calculate Finish\r\n");
    }
}

extern uint8_t current_knot;

bool Dijkstra_TwoZonePathPlanning(int NextKnots) {
    int CurrentKnots, distance, Count = 0;
    // NextKnots = 18;
    CurrentKnots = current_knot;
    memset(PathKnots, -1, sizeof PathKnots);
    distance = dijkstra(CurrentKnots, NextKnots);
    // printf("Current:%d,Next:%d,distance:%d\t Path: ",CurrentKnots,NextKnots,distance);
    // printf("Current:%d,Next:%d,distance:%d\r\n", CurrentKnots, NextKnots, distance);
    for (int i = 0; PathKnots[i] >= 0; ++i) {
        // printf(" %d ", PathKnots[i]);
        Count++;
    }
    // printf("\r\n");
    Dijkstra_CountKnots = Count;
    Count = 0;
    Knots_Num = 0;
    Loop_Num = 0;
    return true;
}
